#include "MoveToDecoyPrePositionState.h"
#include "DecoyEvasiveManeuverState.h"
#include "PDController.h"

bool MoveToDecoyPrePositionState::decoy_is_maneuvering = false;

MoveToDecoyPrePositionState::MoveToDecoyPrePositionState(void)
{
	target = new AttractiveField(Vector2(200, 75));
}


MoveToDecoyPrePositionState::~MoveToDecoyPrePositionState(void)
{
	delete target;
}

Command MoveToDecoyPrePositionState::GetCommand(MultiAgent* agent) {
	Vector2 tank_pos(agent->getTank()->pos[0], agent->getTank()->pos[1]);
	Vector2 trajectory = target->get_trajectory(tank_pos);

	if (trajectory.magnitude() < 385) {
		SetDecoyManeuvaring(true);
	}
	
	 // If we have moved into position to begin maneuvers
	if (trajectory.magnitude() < 50) {
		agent->setState(new DecoyEvasiveManeuverState());
		return agent->GetCommand();
	}

	return PDController::instance()->GetComand(agent->getTank(), trajectory);
}


bool MoveToDecoyPrePositionState::IsDecoyManeuvaring() {
	return decoy_is_maneuvering;
}

void MoveToDecoyPrePositionState::SetDecoyManeuvaring(bool isManeuvaring) {
	decoy_is_maneuvering = isManeuvaring;
}
